/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    Hardware_Node.cpp
 * @author
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include "ros_network/Hardware_Node.h"
#include "common/cppTypes.h"
#include "common/ImuData.h"
#include "common/Math/orientation_tools.h"
#include "tf/tf.h"
#include <exception>

void Hardware_Node::Node_Init(void) {
    ImuData_Puber = n->advertise<common::ImuData>("/quadruped/control/imu_data", 10);
    LegData_Puber = n->advertise<common::LegData>("/quadruped/control/leg_data", 10);
    AU_Suber = n->subscribe("/quadruped/control/au_cmd", 100, &Hardware_Node::AU_Callback, this);
    RobotCmd_Suber = n->subscribe("/quadruped/control/command", 100, &Hardware_Node::RobotCmd_Callback, this);
    IMUData_Suber = n->subscribe("/wit/imu", 100, &Hardware_Node::IMUData_Callback, this);
}

void Hardware_Node::Run(void) {
    LegData data;
    ros::AsyncSpinner s(2);
    s.start();

    // 开启串口传输
    rc_usart.start();

    // 循环等待
    ros::Rate rate(500);

    // test
    RobotCommand init_cmd;
    for (int i = 0; i < 4; i++) {
        init_cmd.flags[i] = 2;
    }
    while (ros::ok()) {
        try{
            // Update robot data from usart
            rc_usart.updateData(&data);
            // int i = 0;

            if (rc_usart.stm32InitFlag[0]&&rc_usart.stm32InitFlag[1]) {
                Publish_LegData(&data);     
            } else {
                std::cout << "wait init" << std::endl;
                rc_usart.updateCommand(&init_cmd);
            } 
            rate.sleep();
        }
        catch(std::exception& e){
            ROS_ERROR("Hardwares Fault!");
        }
    }
    ros::shutdown();
}

void Hardware_Node::Publish_LegData(LegData* data) {
    common::LegData _data;
    convertToMsg(data, &_data);
    LegData_Puber.publish(_data);
}

void Hardware_Node::RobotCmd_Callback(const common::RobotCommandConstPtr& msg) {
    convertFromMsg(msg, &_cmd);
    if(Cmd_Available(_cmd)){
        try{
            rc_usart.updateCommand(&_cmd);
        }
        catch(std::exception& e){
            printf("write false\n");
        }
    }
}

void Hardware_Node::IMUData_Callback(const sensor_msgs::ImuConstPtr& msg) {
    common::ImuData data;
    Vec3<double> rpy;
    Quat<double> _quat;
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg->orientation, quat);
    tf::Matrix3x3(quat).getRPY(rpy(0), rpy(1), rpy(2));
    _quat = ori::rpyToQuat(rpy);
    data.quat.w = _quat.w();
    data.quat.x = _quat.x();
    data.quat.y = _quat.y();
    data.quat.z = _quat.z();

    data.accel[0] = msg->linear_acceleration.x;
    data.accel[1] = msg->linear_acceleration.y;
    data.accel[2] = msg->linear_acceleration.z;

    data.gyro[0] = msg->angular_velocity.x;
    data.gyro[1] = msg->angular_velocity.y;
    data.gyro[2] = msg->angular_velocity.z;
    ImuData_Puber.publish(data);
}

void Hardware_Node::AU_Callback(const std_msgs::Int8ConstPtr& msg){
    rc_usart.updateAU((AU_Type)msg->data);
}

bool Hardware_Node::Cmd_Available(const RobotCommand &cmd){
    for(int leg =  0; leg < 4; leg ++){

        // tau
        if(cmd.tau_abad_ff[leg] > 2. || cmd.tau_abad_ff[leg] < -2.) return false;
        if(cmd.tau_hip_ff[leg] > 12. || cmd.tau_hip_ff[leg] < -12.) return false;
        if(cmd.tau_knee_ff[leg] > 12. || cmd.tau_knee_ff[leg] < -12.) return false;
    }
    return true;
}

int main(int argc, char** argv) {
    // 节点初始化
    ros::init(argc, argv, "Hardware_Node", ros::init_options::AnonymousName);
    ros::NodeHandle n;

    // 创建控制器
    Hardware_Node hw_node(&n, "/dev/DOG_USART1", "/dev/DOG_USART2");
    hw_node.Run();

    return 0;
}

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
